#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>

//Constant params
const float camera_factor = 1000;
const float camera_cx = 256.174;
const float camera_cy = 202.165;
const float camera_fx = 365.674;
const float camera_fy = 365.674;

/**
 * @brief The main process of the programme, working on the convert between depth image and .pcd file.
 * @return
 * @author 1160300719 殷浩然
 */
int main()
{
    cv::Mat depth;
    depth = cv::imread("../data/depth.png", -1);
    pcl::PointCloud<pcl::PointXYZ> cloud;
    for(int i = 0; i < depth.rows; i++){
        for(int j = 0; j < depth.cols; j++){
            ushort d = depth.ptr<ushort>(i)[j];
            if(d == 0){
                continue;
            }
            pcl::PointXYZ p;
            p.z = d / camera_factor;
            p.x = (j - camera_cx) * p.z / camera_fx;
            p.y = (i - camera_cy) * p.z / camera_fy;
            cloud.push_back(p);
        }
    }
    cloud.height = 1;
    cloud.width = (int)cloud.points.size();
    cloud.is_dense = false;
    pcl::io::savePCDFile("../pcd/depth.pcd", cloud);
    cloud.clear();
    return 0;
}